#ifndef FRUIT_DETECTOR_HPP_
#define FRUIT_DETECTOR_HPP_

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <std_msgs/msg/int32.hpp>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.hpp>
#include <vision_msgs/msg/detection2_d_array.hpp>
#include <geometry_msgs/msg/point.hpp>
#include <chrono>
#include "fruit_detector/openvino_detect.hpp"

// 包含自定义消息头文件
#include "fruit_detector/msg/detection_array.hpp"
#include "fruit_detector/msg/fruit_info.hpp"

namespace fruit_detector {

class FruitDetector : public rclcpp::Node {
public:
  explicit FruitDetector(const rclcpp::NodeOptions & options);
  virtual ~FruitDetector() = default;

private:
  // 类型别名
  using DetectionArrayMsg = msg::DetectionArray;
  // 左相机图像订阅回调
  void leftImageCallback(const sensor_msgs::msg::Image::SharedPtr msg);
  
  // 右相机图像订阅回调
  void rightImageCallback(const sensor_msgs::msg::Image::SharedPtr msg);
  
  // detect_part话题订阅回调
  void detectPartCallback(const std_msgs::msg::Int32::SharedPtr msg);
  
  // 处理图像并进行推理
  void processImage(const sensor_msgs::msg::Image::SharedPtr msg, 
                   const std::unique_ptr<yolo::Inference>& inference,
                   const rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr& image_publisher,
                   const rclcpp::Publisher<DetectionArrayMsg>::SharedPtr& detection_publisher);
  
  // 创建水果信息消息
  DetectionArrayMsg createFruitInfoMessage(
    const std::vector<std::vector<cv::Point2f>>& detections,
    const std_msgs::msg::Header& header,
    int img_width,
    int img_height
  );
  
  // 参数
  std::string left_image_topic_;
  std::string right_image_topic_;
  std::string model_path_a_;
  std::string model_path_b_;
  std::string model_path_c_;
  float conf_threshold_;
  float iou_threshold_;
  
  // 当前的detect_part值（1, 2, 或 3）
  int current_detect_part_;
  
  // OpenVINO推理器 - 三个模型（A, B, C），左右相机各一个
  std::unique_ptr<yolo::Inference> left_inference_a_;
  std::unique_ptr<yolo::Inference> left_inference_b_;
  std::unique_ptr<yolo::Inference> left_inference_c_;
  std::unique_ptr<yolo::Inference> right_inference_a_;
  std::unique_ptr<yolo::Inference> right_inference_b_;
  std::unique_ptr<yolo::Inference> right_inference_c_;
  
  // ROS订阅和发布
  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr left_image_subscription_;
  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr right_image_subscription_;
  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr detect_part_subscription_;
  
  // 左相机发布器
  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr left_image_publisher_;
  rclcpp::Publisher<DetectionArrayMsg>::SharedPtr left_detection_publisher_;
  
  // 右相机发布器
  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr right_image_publisher_;
  rclcpp::Publisher<DetectionArrayMsg>::SharedPtr right_detection_publisher_;
  
  // FPS计算相关
  std::chrono::time_point<std::chrono::steady_clock> last_left_frame_time_;
  std::chrono::time_point<std::chrono::steady_clock> last_right_frame_time_;
  float left_current_fps_;
  float right_current_fps_;
  bool left_first_frame_;
  bool right_first_frame_;
};

} // namespace fruit_detector

#endif // FRUIT_DETECTOR_HPP_ 